/**
 * @file:          TrackingDifferentiator.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.10.16
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.10.16,19:40:17
 */

/* Include Files **************************************************************/
#include "TrackingDifferentiator.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void TrackingDifferentiator_Init(struct TrackingDifferentiator *self)
{
    self->UpdateParam = TrackingDifferentiator_UpdateParam;

    switch (self->param_mode)
    {
    case TDMode_Fhan:
        self->Run = TrackingDifferentiator_Run_Fhan;
        break;
    case TDMode_Linear:
        // self->Run = TrackingDifferentiator_Run_Linear;
        break;
    default:
        break;
    }

    self->fhan.Init(&self->fhan);
    self->UpdateParam(self);
}

float TrackingDifferentiator_Run_Fhan(struct TrackingDifferentiator *self)
{
    float out = 0;

    if (self->param_en != 0)
    {
        self->v1 = self->v1_next;
        self->v2 = self->v2_next;

        self->fhan.in_x1 = self->v1 - self->in_v;
        self->fhan.in_x2 = self->v2;

        if (self->param_single_turn_mode != 0)
        {
            if (self->fhan.in_x1 > BM_PI)
            {
                self->fhan.in_x1 = self->v1 - (self->in_v + BM_2PI);
            }
            else if (self->fhan.in_x1 < -BM_PI)
            {
                self->fhan.in_x1 = (self->v1 + BM_2PI) - self->in_v;
            }
        }

        self->y = self->fhan.Run(&self->fhan);
        self->v1_next = self->v1 + self->param_h * self->v2;
        self->v2_next = self->v2 + self->param_h * self->y;

        // self->v1_next = fmodf(self->v1_next, BM_2PI);
        if (self->param_single_turn_mode != 0)
        {
            if (self->v1_next >= BM_2PI)
            {
                self->v1_next -= BM_2PI;
            }
            else if (self->v1_next < 0)
            {
                self->v1_next += BM_2PI;
            }
        }

        self->out_x1 = self->v1;
        self->out_x2 = self->v2 * self->param_d_gain;
        self->out_g = self->y * self->param_dd_gain;
    }
    else
    {
        self->out_x1 = self->in_v;
        self->out_x2 = 0.0F;
        self->out_g = 0.0F;
    }

    return out;
}

float TrackingDifferentiator_Run_Linear(struct TrackingDifferentiator *self)
{
    float out = 0;

    if (self->param_en != 0)
    {
        self->v1 = self->v1_next;
        self->v2 = self->v2_next;

        self->fhan.in_x1 = self->v1 - self->in_v;
        self->fhan.in_x2 = self->v2;

        if (self->param_single_turn_mode != 0)
        {
            if (self->fhan.in_x1 > BM_PI)
            {
                self->fhan.in_x1 = self->v1 - (self->in_v + BM_2PI);
            }
            else if (self->fhan.in_x1 < -BM_PI)
            {
                self->fhan.in_x1 = (self->v1 + BM_2PI) - self->in_v;
            }
        }

        self->y = -self->fhan.r * self->fhan.r * self->fhan.in_x1 - 2 * self->fhan.r * self->fhan.in_x2;
        self->v1_next = self->v1 + self->param_h * self->v2;
        self->v2_next = self->v2 + self->param_h * self->y;

        // self->v1_next = fmodf(self->v1_next, BM_2PI);
        if (self->param_single_turn_mode != 0)
        {
            if (self->v1_next >= BM_2PI)
            {
                self->v1_next -= BM_2PI;
            }
            else if (self->v1_next < 0)
            {
                self->v1_next += BM_2PI;
            }
        }

        self->out_x1 = self->v1;
        self->out_x2 = self->v2 * self->param_d_gain;
        self->out_g = self->y * self->param_dd_gain;
    }
    else
    {
        self->out_x1 = self->in_v;
        self->out_x2 = 0.0F;
        self->out_g = 0.0F;
    }

    return out;
}

float TrackingDifferentiator_UpdateParam(struct TrackingDifferentiator *self)
{
    self->fhan.param_h = self->param_h * self->param_h0_gain_h;
    self->fhan.UpdateParam(&self->fhan);
}